Path Planning 
through 

Variational Dynamic Programming 



September 1 993 



Jerome Barraquand 
Pierre Ferbach 



0 



Path Planning 
through 

Variational Dynamic Programming 



Jerome Barraquand 
Pierre Ferbach 



September 1993 



Publication Notes 



The authors can be contacted at the following addresses: 



Jerome Barraquand 
Digital Equipment Corporation 
Paris Research Laboratory 
85, Avenue Victor Hugo 
92500 Rueil-Malmaison, France 
barraquand@prl . dec . com 



Pierre Ferbach 

Ecole Nationale Superieure 

des Techniques Avancees 

32, Bd Victor 

75015 Paris, France 

f erbach@ensta . f r 



© Digital Equipment Corporation 1994 



This work may not be copied or reproduced in whole or in part for any commercial purpose. Permission 
to copy in whole or in part without payment of fee is granted for non-profit educational and research 
purposes provided that all such whole or partial copies include the following: a notice that such copying 
is by permission of the Paris Research Laboratory of Digital Equipment Centre Technique Europe, in 
Rueil-Malmaison, France; an acknowledgement of the authors and individual contributors to the work; 
and all applicable portions of the copyright notice. Copying, reproducing, or republishing for any other 
purpose shall require a license with payment of fee to the Paris Research Laboratory. All rights reserved. 

ii 



Abstract 



The path planning problem, i.e., the geometrical problem of finding a collision-free path 
between two given configurations of a robot moving among obstacles, has been studied by 
many authors in recent years. Several complete algorithms exist for robots with few degrees 
of freedom (DOF), but they are intractable for more than 4 DOE In order to tackle problems in 
higher dimensions, several heuristic approaches have been developed for various subclasses of 
the general problem. The most efficient heuristics rely on the construction of potential fields, 
attracting the robot towards its goal configuration. However, there is no obvious way to extend 
this approach to manipulation task planning problems. 

This report presents a novel approach to path planning which does not make use of a potential 
function to guide the search. It is a variational technique, consisting of iteratively improving 
an initial path possibly colliding with obstacles. At each iteration, the path is improved 
by performing a dynamic programming search in a submanifold of the configuration space 
containing the current path. We call this method Variational Dynamic Programming (VDP). 
The method can solve difficult high-dimensional path planning problems without using any 
problem-specific heuristics. Experiments are reported for several computer simulated robots 
in 2D and 3D workspaces, including manipulator arms and mobile robots with up to 16 
DOFs. More importantly, an extension of VDP can solve manipulation planning problems of 
unprecedented complexity. We report an experiment in dual-arm manipulation planning with 
12 DOF in a cluttered workspace. 



Resume 



Le probleme de la planification de trajectoire, i.e., le probleme geometrique consistant a trouver 
des chemins sans collision entre deux configurations d'un robot en presence d'obstacles, a ete 
largement etudie ces dernieres annees. De nombreux algorithmes existent pour resoudre ce 
probleme dans des cas pratiques. Toutefois, l'extension de ces methodes aux problemes de 
planifications de taches de manipulation n'est pas triviale. 

Nous presentons dans ce rapport une methode variationnelle, consistant a ameliorer itera- 
tivement un chemin initial pour eviter une collision eventuelle avec les obstacles. A chaque 
iteration, le chemin est ameliore en realisant une recherche par programmation dynamique dans 
une sous variete de l'espace des configurations contenant le chemin courant. Nous appelons 
cette methode Programmation Dynamique Variationnelle (PDV). La methode peut resoudre 
des problemes difficiles de planification de trajectoire en dimension elevee sans recourir a des 
heuristiques specifiques au probleme considere. De plus, une extension de PDV peut resoudre 
des problemes de planification de manipulation d'une complexite sans precedent. 
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1 Introduction 

We present a new method for geometrical path planning with many DOF. This method, unlike 
other planning methods for many DOF, does not require problem-specific heuristics such as 
potential functions to guide the search. The method was initially developed for the basic path 
planning problem in open free space, but its capabilities extend to several other instances of the 
more general constrained motion planning problem. In particular, an extension of the method 
can solve complex manipulation task planning problems. It is a variational technique, consisting 
of iteratively improving an initial path possibly colliding with obstacles. The originality of our 
method is to depart from standard gradient-based variational calculus techniques. Instead, at 
each iteration, we perturb the current path by performing a dynamic programming search in 
a ^-dimensional submanifold of the ^-dimensional configuration space containing the current 
path. In practice, k is chosen equal to 2, 3, or 4 in order to make the dynamic programming 
search tractable. Thanks to this dynamic programming strategy, the algorithm can avoid in 
many cases spurious local minima of the cost functional. Furthermore, when local minima 
arise, the result of the dynamic programming search can be used to adequately modify the cost 
functional, by the introduction of additional repulsion points around colliding zones on the 
path. This enables the algorithm to get out of the most difficult local minima. 

The ^-dimensional submanifold is an arbitrarily chosen ruled surface containing the current 
path. This surface is quantized into a ^-dimensional grid of configurations. Then, the grid is 
searched using Dijkstra's algorithm with an additive cost function proportional to the number 
of configurations colliding with obstacles. Thus, it is guaranteed that fewer points in the new 
path collide with obstacles. Then, the operation is repeated until a free path is found. We 
call this method Variational Dynamic Programming (VDP). The idea behind VDP is to use as 
much as possible the power of classical complete dynamic programming-based methods, while 
avoiding their exponential memory and time requirements. 

We have implemented this approach in a fully functional simulation program, and conducted 
extensive tests. Experiments are reported for several computer simulated robots in 2D and 3D 
workspaces, including manipulator arms and mobile robots with up to 16 DOF. To the best of 
our knowledge, only potential field based methods can solve problems of similar complexity. 
The specificity of VDP is that it can solve difficult high-dimensional planning problems without 
using any problem-specific heuristics. This is in itself an important point for future research in 
geometrical planning. It demonstrates that cluttered high-dimensional spaces can be practically 
searched without relying on any problem-specific knowledge. One major implication of this 
result is that VDP can be generalized for solving complex manipulation planning problems. 
This is to be contrasted with potential field based methods, which require problem-specific 
heuristics to resolve such problems. Of course, the generality of the method is obtained at 
some cost: the planner is considerably slower than some potential field-based methods, in 
particular the RPP method described in Barraquand and Latombe 1991 [5]. 

In order to explore the flexibility of the VDP approach, we have attempted to imbed into the 
planner some heuristic information about the topology of the workspace. More precisely, 
instead of applying the VDP method directly on the input workspace, we first generate a series 
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of more and more cluttered workspaces, the first being virtually free of obstacles, and the last 
being the original input workspace. Then, we progressively apply the VDP method to the series 
of workspaces. The input path used in the VDP algorithm for a given workspace in the series is 
the output path of the VDP method applied to the previous less cluttered workspace. In order to 
speed up the algorithm, the dynamic programming search at each iteration is only conducted in 
a small neighborhood of the current path. This makes the method considerably faster, although 
less general in theory. The idea is that the solution paths for two similar workspaces should be 
relatively close to one another in many cases. We call this version of the planner Progressive 
Variational Dynamic Programming (PVDP). The resulting planner is less general in theory than 
the original VDP planner, since it uses problem-specific heuristics to guide the search. On the 
other hand, it is dramatically faster. In fact, it can solve some problems in a time comparable 
to that of potential-field based methods. 

PVDP can be used to address constrained motion planning problems, i.e., extensions of the 
basic path planning problem where the free space in not necessarily an open subset of the 
configuration space. In particular, we have successfully applied PVDP to high-dimensional 
manipulation planning problems. We briefly describe below the extension of the PVDP method 
to manipulation planning problems. A complete presentation of the method can be found in 
Ferbach and Barraquand 1993 [16]. Given an environment containing a robot, stationary 
obstacles, and movable bodies, the manipulation problem consists in finding a sequence of free 
robot motions, grasping and ungrasping operations, to reach a given state from a given initial 
state in the joint configuration space of the robot and all movable bodies. The movable objects 
can only move when they are grasped by the robot. The generalized obstacles (i.e., forbidden 
postures) in the joint configuration space C are not only the configurations where the robot 
or the movable objects hit the stationary obstacles, but also all postures where the movable 
objects are levitating without being grasped by the robot. Hence, the free space of the joint 
system is not anymore an open subset of the configuration space manifold. In particular, at 
a configuration q where the robot grasps one object, the free space in the neighborhood of q 
is an (n — h) -dimensional submanifold of the n -dimensional configuration space C, h being 
the number of grasping constraints. The principle underlying PVDP is to replace the equality- 
constrained problem by a convergent series of more and more difficult inequality-constrained 
planning problems in open free space. In other words, grasping constraints are handled by 
PVDP in an iterative fashion. PVDP first computes a path where the movable objects can 
levitate without being grasped by the robots. Then, this path is used as the input for a series 
of increasingly difficult problems where the objects must get closer and closer to the robots 
in order to move. The planner has successfully solved manipulation planning problems of 
unprecedented complexity. In particular, we report an experiment in dual-arm manipulation 
task planning for a 12 DOF system. Several other examples are described in Ferbach and 
Barraquand 1993 [16]. 

This report is organized as follows. In Section 2, we relate our contribution to previous work in 
motion planning. In Section 3, we discuss the representational issues for geometric primitives 
relevant to the path planning problem, and more specifically to the collision detection problem. 
In Section 4 we describe the general principle underlying Variational Dynamic Programming. 
In Section 5 we present the faster heuristic version of the planner PVDP. In Section 6, we 
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present experimental results illustrating the capabilities of the implemented planners. In 
section 7, we briefly discuss some theoretical and practical issues related to variational dynamic 
programming. 

2 Relation to other work 

The path planning problem, i.e., the geometrical problem of finding a collision-free path 
between two given configurations of a robot moving among obstacles, has been much studied in 
recent years (Latombe 1990 [24]). Today the mathematical and computational structures of the 
general problem (when stated in algebraic terms) are reasonably well understood (Schwartz and 
Shark 1983 [32]) (Canny 1988 [1 1]). In addition, practical algorithms have been implemented 
in more or less specific cases, e.g., (Brooks and Lozano-Perez 1983 [9]) (Faverjon 1984 [14]) 
(Lozano-Perez [27]) (Faverjon and Tournassoud 1987 [15]) (Zhu and Latombe 1991 [35]) 
(Barraquand Langlois and Latombe 1992 [4] ). 

Many efficient and complete algorithms exist when the number of degrees of freedom (DOF) 
of the robot is small (Latombe 1990 [24]): exact or approximate cell decomposition methods, 
roadmap methods, grid search methods. These methods differ mostly in the data representa- 
tions used to construct the connectivity graph of the free space. But they all rely on the same 
general algorithmic principle for searching the connectivity graph: Dynamic Programming. 
Sometimes, heuristics are imbedded to speedup the search, and various algorithms such as A* 
or Best First Search are used instead of Breadth First algorithms such as Dijkstra's. These 
algorithms nevertheless use variants of the Bellman principle of Dynamic Programming, as 
exemplified in Bertsekas 1988 [8]. Hence, they suffer from the traditional "curse of dimension- 
ality" problem of Dynamic Programming (Bellman 1958 [7]): they require exponential space 
and time in the number of DOF. These methods are therefore intractable for more than 4 DOF. 
This is not surprising, since these methods are complete, while the path planning problem is 
known to be PSPACE-hard. 

In order to tackle problems in higher dimensions, several heuristic (i.e., incomplete) approaches 
have been developed for various subclasses of the general problem, and some successful systems 
have been implemented, e.g., (Donald 1984 [12]) and (Faverjon and Tournassoud 1987 [15]). 

Variational techniques, i.e., techniques consisting of improving an initial path possibly colliding 
with obstacles, have already been used in an earlier work on path planning (e.g., Buckley 
1985 [10], Gilbert and Johnson 1985 [17], Dupont and Derby 1988 [13], Warren 1989 [33]). 
In its original form, variational planning suffers from a severe drawback. Indeed, since it 
usually consists of minimizing a cost function along its negated gradient by means of standard 
variational calculus methods, it gets stuck in most realistic cases in a local minimum of the cost 
functional that does not correspond to a free path. In addition, the optimization of the functional 
is conducted over the space of all possible paths, and can be quite computationally intensive. To 
the best of our knowledge, no robust planning method based solely upon variational techniques 
has been developed to date. Variational Dynamic Programming is not a gradient-based method, 
hence does not suffer from the same drawbacks. 
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A widely used heuristic consists in guiding the robot along the negated gradient of a real-valued 
function defined over the configuration space, called the potential function. The potential has 
two components: a goal potential attracting the robot towards its goal configuration, and an 
obstacle potential, repulsing the robot from the obstacles. This so-called artificial potential 
field approach was originally proposed in Khatib 1986 [20]. Emphasis was put on real-time 
efficiency, rather than on completeness. In particular, since it acts as a gradient descent 
optimization procedure, this approach may get stuck at a local minimum of the potential 
function. The local-minima problem can be addressed at two levels: (1) in the definition of the 
potential function, by attempting to specify a function with no or few local minima; and (2) 
in the design of the search algorithm, by including appropriate techniques for escaping from 
local minima. At the first level, the construction of analytical potentials free of local minima 
has been investigated, so far with limited success. Solutions have been proposed only in 
Euclidean configuration spaces with spherical or star-shaped obstacles (Koditschek 1987 [21]) 
(Rimon and Koditschek 1989 [31]). Another line of research has been to construct numerical 
potential functions with "good" properties (Barraquand Langlois and Latombe 1992 [4] ). At 
the second level, powerful methods have been developed for escaping from local minima, in 
particular randomization methods (Barraquand and Latombe 1991 [5]). Very recently, new and 
promissing randomization methods have been developed by Overmars 1992 [30] and Kavraki 
and Latombe 1993 [19]. 

Potential field methods appear to outperform other approaches for practical path planning 
problems with many degrees of freedom. In particular, the RPP method described in Barraquand 
and Latombe 1991 [5] is already being used in industrial settings (Ohlund 1990 [29]), (Graux 
et al. 1992 [18]). However, the efficiency of these methods highly depends on the properties 
of problem-specific potential functions. In particular, extending the capabilities of potential 
field-based planners to more general manipulation task planning problems is a difficult task. 

The interest in manipulation task planning is more recent in the robotics literature. The problem 
of planning the path of a convex polygonal robot translating in a two-dimensional polygonal 
workspace in the presence of multiple convex polygonal movable objects is addressed in 
Wilfong 1988 [34]. The general manipulation problem is described in a series of papers from 
Alami, Laumond, and Simeon (Alami Simeon and Laumond 1989 [2], Laumond and Alami 
1989 [25]). An implemented algorithm for a 2 DOF robot grasping a single object at a time 
and several 2 DOF bodies translating in the plane is presented in Alami Simeon and Laumond 
1989 [2]. The planner has two components: a classical path planner, and a manipulation 
task planner (MTP). The MTP plans a sequence a robot motions, grasping and ungrasping 
operations, and transfer motions (i.e., motions of the robot together with a grasped object). 
The approach is practically limited to non-redundant robots with few DOF, and requires an 
exhaustive exploration of the robot's configuration space. Koga and Latombe 1992 [22] present 
several implemented planners solving various dual-arm manipulation planning problems of 
increasing difficulty. They use and extend the framework of Alami Simeon and Laumond 1989 
[2]. The planner is again the combination of a path planner and a manipulation task planner. 
An extension of this approach yielding impressive experimental results is presented in Koga 
and Latombe 1993 [23]. 
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Our approach to manipulation task planning is fundamentally different. We do not decompose 
the problem into a sequence of robot motions and manipulation tasks. Our planner is not a 
combination of a path planner and a manipulation task planner. Instead, we simply consider 
the whole manipulation problem as a special instance of the basic path planning problem in the 
joint configuration space of the robot and the movable objects. The major advantage of this 
approach is to avoid the artificial decoupling between motion planning and task planning. As a 
consequence, PVDP can solve manipulation planning problems of unprecedented complexity. 

3 Centralized versus Distributed Representations 

3.1 Definitions 

Let A denote the robot, W its workspace, and C its configuration space. A configuration of the 
robot, i.e., a point in C, completely specifies the position of every point in A with respect to a 
coordinate system attached to W (Lozano-Perez 1983 [26]). Let n be the dimension of C, i.e., 
the number of DOE We represent a configuration q G C by a list of n parameters {q\, q n ), 
with appropriate modulo arithmetic for the angular parameters (Latombe 1990 [24]). The 
subset of C consisting of all the configurations where the robot has no contact or intersection 
with the obstacles in W is called the free space and is denoted by Cf ree . 

For each point p G A, one can consider the geometrical application that maps any configuration 
q = (qi, q n ) G C to the position w G W of p in the workspace. This map: 

X : A x C W 

(p,q) ^ X(p,q) = w 

is called forward kinematic map. 

3.2 Centralized Representations: the Problem of Collision Detection 

Most solid modeling systems used in scientific computing or computer aided design represent 
geometric primitives by algebraic inequalities defining the boundaries of objects. This is 
also the case of systems used for the generation of computer graphics scenes. Often, the 
algebraic inequalities used are linear, and the geometric primitives are simply polyhedra. 
Representations of this kind are called centralized representations. The great advantage of 
centralized representations is that they provide a precise description of objects boundaries at 
any scale, while minimizing the amount of redundant information. Using such representations, 
accurate modeling of 3D structures can fit into the memory of current computer workstations. 
However, these representations have a severe drawback. They are unstructured, i.e., assessing 
the occupancy of a given location in space requires scanning the list of objects present in the 
scene. Therefore, detecting the collision of a given point in space with the objects present 
in the scene requires a time linear in the number of geometric primitives. Through the use 
of hierarchical representations such as octrees, the assessment of relative positions of static 
objects in the scene can be made much faster. Unfortunately, octree decompositions are not 
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practical when some objects are movable, since they may change dramatically under small 
displacements. 

The high computational requirements of motion planning are mostly due to the need to perform 
repeated collision checking between the robot and the obstacles (Metivier and Urbschat 1990 
[28]). Detecting the collision of a robot with many DOF in realistic environments may take as 
much as 1/10 to 1 second when using centralized representations. Planning of a path requires 
a number of collision detections ranging from a few hundred for the simplest cases to a few 
hundreds of thousands for the most complex ones. Such computation times are practically 
prohibitive for planning very complex motions using centralized representations. 

3.3 Distributed Representations 

The experiments reported in this paper were all performed using a distributed representation 
of the workspace. The workspace W is modeled as a A^-dimensional bitmap array, with N = 2 
or 3 being the dimension of W. The array is defined by the following function BM: 



BM : W — > {1,0} 
w i y BM{w) 



in such a way that the subset of points w such that BM{w) = 1 represents the workspace 
obstacles and the subset of points w such that BM(w) = 0 represents the empty part of the 
workspace. We write: H 'empty = {w eW, BM(w) = 0}. 

The main advantage of distributed representations is that they are structured, i.e., assessing 
the occupancy of any point in workspace is performed in a time constant in the number and 
shape of the obstacles, and in the resolution of the bitmap. A point x is occupied if and only if 
BM{x) = 1. Consequently, checking the collision of the robot with obstacles can be done by 
simply "drawing" the robot on the bitmap. The drawing procedures used are reminiscent of the 
Bresenham's algorithm well known in Computer Graphics literature. Details on the collision 
detection methods employed can be found in (Barraquand and Latombe 1991 [5]). 

The drawback of distributed representations is the high memory requirement associated with 
the bitmap array, especially for 3D workspaces. In the experiments, the resolution used was 
256 2 for 2D workspaces, and 128 3 for 3D workspaces. In order to store high resolution 
3D bitmaps on current workstations, it is necessary to compress the bitmap. Indeed, some 
industrial settings require a resolution of the order of 1000 3 . Corresponding bitmaps arrays do 
not fit in the memory of current low cost computer workstations without compression. Strong 
compression ratios can be obtained by using an octree or a runlength coding technique for one 
of the spatial dimensions. However, assessing occupancy over the compressed representation 
is no longer constant in the resolution of the bitmap. Collision checking is typically one order 
of magnitude slower for such compressed representations. 
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4 Variational Dynamic Programming 

In this section we describe the Variational Dynamic Programming (VDP) method. It is based 
upon a dynamic programming technique applied successively to various submanifolds of the 
configuration space. The idea behind VDP is to use as much as possible the power of classical 
complete dynamic programming-based methods, while avoiding their exponential memory and 
time requirements. In order to generate a free path in a configuration space of much higher 
dimension, VDP conducts iteratively several searches in 2 or 3-dimensional submanifolds of 
the configuration space. 

4.1 General VDP algorithm 

The input to the algorithm is: 

• The initial configuration q im - f 

• The goal configuration q goa i 

• The specification of the forward kinematic map and the distribution of obstacles in the 
workspace. In the current implementation, the workspace in represented as a bitmap as 
described in Section 3. However, the algorithm below is independent of the chosen data 
representation. 

The output of the algorithm at any given iteration is a path lying as much as possible in free 
space. The total number of iterations is arbitrarily bounded to a prespecified number. The 
algorithm terminates if a free path is found at a given iteration. Otherwise, the algorithm returns 
the best available path obtained after the prespecified number of iterations. 

The general VDP algorithm can be described as follows. 
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algorithm VDP (VARIATIONAL DYNAMIC PROGRAMMING) 
begin 

Generation of the initial path; 
while Collision with obstacles 

Generation of submanifold; 

Generation of repulsion points; 

Generation of cost function; 

Generation of minimum cost path within submanifold; 
Reparameterization of path; 
endwhile; 



This algorithm generates iteratively a series of paths joining q,„, f to q goa i, with a decreasing 
percentage of collision points. 

We now describe the various parts of the general algorithm in more detail. 

4.2 Generation of the initial path 

In the current implementation, the initial path 7 is simply a geodesic path (for an appropriate 
metric) between 7(0) = q,„ lf and 7(1) = q goa i in the configuration space manifold. For 
example, if the configuration space is a convex open subset of R", the initial path is the straight 
line joining q/n/f and <\g 0a i- This straight line is quantized into a series of m -\- 1 equally spaced 
configurations, the (i + \) th point being 7 (i/m) = (1 — j'/m)q, m - f + i/mq goa i. The distance d re f 
between two consecutive configurations is chosen small enough so as to induce a small robot 
motion in the workspace (see e.g., Barraquand and Latombe 1991 [5] for a discussion). 

4.3 Collision with obstacles 

This function returns true if the current path collides with obstacles, and false if the current 
path is a free path. More precisely, it examines each discrete point along the path and computes 
the corresponding position of the robot using the forward kinematic map. Then, it tests if this 
position hits obstacles using the collision detection techniques described in Section 3. 

4.4 Generation of the repulsion points 

At a given iteration of the VDP algorithm, the current path 7 collides with obstacles at one or 
more points. We partition the path into a series of connected free and colliding zones. More 
precisely, we compute a subdivision 0 = sq < s\ < . . . < S2 r +i = 1 of the interval [0, 1] 
verifying the following properties: 



end; 



Vi G [0, r],Vj G]*2/,*2/+i[, 



7(5) G C fi 



'ree 



Vi G [0, r 



1] , Ms G [s 2 i+ 1 , s 2 i+2] , 7 (s) & Cfri 



ve 
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For each colliding zone [J2/+1, *2/+2]» i G [0, r — 1], we define a repulsion point rep,- = 
l((s2i+i + s 2(+2)/2) in the middle of the zone. The definition of these r repulsion points 
will be useful for escaping local minima of the overall cost function along the path. We also 
compute for each repulsion point rep, the radius of the corresponding colliding zone: 



where d(q, q') is the Riemanian distance between q and q' for an appropriate metric in the 
configuration space manifold C. In practice, d is the Euclidean distance between the two 
vectors q and q' considered as elements of R n . 

4.5 Generation of the submanifolds 

We describe how a fc-dimensional submanifold of the configuration space containing a given 
path can be constructed. At a given iteration of the VDP algorithm, we have a current path 
j(s) linking 7(0) = q init and 7(1) = q goa/ . 

In a first step, we select two unit vectors v ;mf and v goa i in the generalized coordinate system 
(qi, . . . , q n ). In the absence of reliable heuristic, we select these two vectors randomly using 
a uniform probability distribution on the unit sphere in R". We extend the path 7 at both 
extremities by defining the extended path 7 in the following fashion: 



In general, the extended path 7 can be defined for all s e R, i.e. , it can be prolongated indefinitely 
in both directions. However, we assume that the configuration space is a bounded manifold. 
This is a very reasonable assumption, since any practical robotics system has a bounded range 
of action. Hence, the generalized coordinates q = {q\ , . . . , q n ) stay in a bounded subset of R". 
Therefore, there exist two numbers s m ,„ < 0 < 1 < s max such that all configurations 7(5) for 
s G" [s m in, Smax] are unreachable. 

In a second step, we randomly select a set of k - 1 independent unit vectors u\ , . . . , u^- 1, using 
again a uniform probability distribution on the unit sphere in R n . This enables us to define 
parametrically a ^-dimensional ruled submanifold <S 7 of the configuration space C : 



As it is the case for the first parametric coordinate s, all other parametric coordinates A 1 , . . . , 1 
are bounded, since the configuration space is assumed bounded: 



Ri = -</(7(j 2 ;+i), 7(^+2)) 



Hinit 

+ SVMt if S < 0 

7(5) if s e [0, 1] 

. Qgoal + SV goa l if S > 1 



k-l 




!=1 



Vi G [1,* 



1], A,- G [A,- mm , A,- max ] 
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Hence, the set of parametric coordinates (s, Ai, . . . , Xk-i) is a bounded subset of R k . The 
bounded submanifold <S 7 is then quantized into a finite cartesian grid along its parametric co- 
ordinates s, Ai, . . . , \k-i, using constant increments 5s, 5\\, . . . , ^A^-i. Within the quantized 
submanifold, the set of neighbors of a given configuration q is the classical ^-neighborhood for 
the parametric coordinates, i.e., the set of 3 k - 1 configurations whose parameters differ from 
those of q of one quantization step at most. For notational convenience, we will indifferently 
denote by s or Ao the parameter along the current path. The quantization 5\t along each 
parameter A,- is chosen in such a way that the distance between two neighboring configurations 
is of the order of d re f. 

Remark: The construction of the ^-dimensional submanifold described above can be slightly 
modified in the following fashion. Instead of selecting constant unit vectors v,„ lf and v goa i, we 
can select two series of "slowly" varying vectors Vs < 0, Vi n j t (s) and Vs > 1, v goa i{s) such that 
the difference between two consecutive vectors in the series is "small". Similarly, we can define 
slowly varying series of vectors Ms, w;(s) for each index i £ [1, k - 1]. In our experiments, 
we have implemented both approaches. The experimental performance of the VDP algorithm 
does not seem to be affected by the variability of unit vectors. However, there is an important 
theoretical difference between the two approaches. Indeed, the version using varying unit 
vectors is probabilistically resolution-complete, i.e., if a solution path exists in open free space, 
then the probability of finding a quantized path at distance less than d re f to this solution path 
tends towards one when the computation time tends towards infinity. 

Let us assume that a collision free path j so i exists. If the unit vectors are allowed to vary along 
the coordinate s, it is easily seen that at each iteration, there is a very small but strictly positive 
lower bound p on the probability that the submanifold generated contains a path j' identical 
to j so i up to the configuration space quantization d re f. In this event, 7' is a collision- free path 
in the search submanifold. The algorithm will therefore necessarily find a collision-free path 
thanks to the optimality of Dijkstra's algorithm. We can conclude that the probability of finding 
a solution path after ./V iterations of VDP is lower bounded by 1 — (1 — p) N . Therefore, this 
probability tends towards 1 when the number of iterations tends towards infinity. The rate of 
convergence is geometric. However, the lower bound p is so small in practice that this result 
says little about the actual efficiency of VDP. 

4.6 Generation of the cost function 

The VDP algorithm consists in iteratively improving an initial path by performing dynamic 
programming searches in ^-dimensional submanifold grids. We describe the cost function 
used for the search within a given grid. The total cost along a quantized path ^{sq = 
0), 7(51), . . . , j(s m = 1) is an additive functional: 



m— 1 



^(7) = E C (^')> 7(*+i)) 



i=0 
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The elementary cost function C(q, q') between two neighboring configurations is the product 
of two components: 

C(q, q') = C ohst (q, q') X C rep (q, q') 

The component C 0 b st has higher values in colliding zones, thereby inducing the optimal path to 
lie as much as possible in free space. The component C rep has higher values in the neighborhood 
of repulsion points, thereby forcing the optimal path out of local minima of the pure obstacle- 
avoidance functional Jc ob „ ■ 

We now describe in more detail the expressions of C 0 b st and C rep . 



0.001 X %%fL ifqandq' are non-collision configurations 



<Wq, q') = \ 1 v dJM} 

dref 

where d(q, q') is again the distance between q and q' in configuration space 



1 X ( j e ' if q or q' is a collision configuration 



rep i , . . . , rep r being the r repulsion points precomputed at the current iteration, and R\ , . . . , R r 
the radii of the corresponding colliding zones, the multiplicative cost factor C rep is defined as 
follows. 

C - (q ' q/) = 1 + g ai ^repJ 

where q" = 34^- , and ot\ , «2 , . . . , oi r are positive coefficients chosen at each iteration randomly 
between 0.5 and 2 for example. We call these coefficients the repulsion coefficients. 



4.7 Generation of the minimum cost path within a submanifold 



This procedure achieves the dynamic programming search of an optimal path within the 
quantized submanifold using the standard Dijkstra's algorithm (see e.g., Aho Hopcroft and 
Ullman 1983 [1]). The priority queue is implemented as a heap. 



4.8 Reparameterization of the path 



After an optimal path has been found by the search algorithm, the distance in configuration 
space between two consecutive points along the path is not equal anymore to the reference 
distance d re f. This procedure simply reparameterizes the path in such a way that the distance 
between two consecutive points equals d re f. 



5 Progressive Variational Dynamic Programming 
5.1 Reducing the size of the search space 

The experiments presented in Section 6 show that VDP is a powerful path planner. Indeed, it can 
solve very difficult planning problems in cluttered workspaces with robots having many DOE 
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We have found that VDP can solve all the problems that have been solved by the potential field 
based planner RPP (Barraquand and Latombe 1991 [5]). However, in its original form, VDP 
is about two orders of magnitude slower than RPP for the most difficult problems. This can be 
easily understood, since at each iteration of the algorithm, VDP performs an uninformed search 
(Dijkstra's algorithm) of the whole ^-dimensional array of quantized parametric coordinates 
Ao, • • •, Afc_i. 

By reducing the size of this search space at each iteration, the total computation time can be 
dramatically reduced. Let 7, be the path at the end of iteration i of the VDP algorithm. If the 
path 7, is already close to a collision free-path, the optimal path 7, + i obtained after the search 
of the whole ^-dimensional submanifold at iteration i + 1 lies in a small neighborhood of 7,-. 
Hence, a solution for dramatically reducing the number of explored cells is to limit the search 
for 7, + i to a small "tubular" neighborhood of 7,-. This will work if the configuration space is 
not too cluttered, i.e., if the motion planning problem at hand is simple. 

In order to use the same idea for more difficult problems, a solution is to replace the initial 
motion planning problem by a series a simpler problems in less cluttered workspaces converging 
towards the initial problem. More precisely, instead of applying the VDP method directly on 
the input workspace, we can first generate a series of more and more cluttered workspaces 
using heuristic ad-hoc techniques, the first being virtually free of obstacles, and the last being 
the original input workspace. Then, we progressively apply the VDP method to the series of 
workspaces. The input path used in the VDP algorithm for a given workspace in the series is 
the output path of the VDP method applied to the previous less cluttered workspace. Since 
two consecutive problems in the series are similar, it can be expected that the solution paths 
for those two problems will also be similar. Hence, the dynamic programming search at each 
iteration can be only conducted in a small neighborhood of the current path. This idea of 
Progressive Variational Dynamic Programming is described in more detail below. 

5.2 Progressive Variational Dynamic Programming 

Let P be our initial motion planning problem, consisting in finding a path 7 joining 7 (0) = q !m - f 
and 7(1) = q g oai while avoiding obstacles: 

VSE [0,1], 7 (s) eCfree 

We can define in many different ways (see next subsection) a decreasing finite sequence of 
free-spaces C D C^ ree D Cj ree D . . . D C^ ree D . . .Cft™ = Cf ree . Then, we can replace problem 
P by the sequence of problems P, whose solution paths 7,- must satisfy the simpler obstacle 
avoidance constraints: 

toe [0,1], jitfecjte 

The original VDP algorithm can be reparameterized to better fit the need of each subproblem 
Pi- 

VDP(Cf ree , k, X k max , nbi ter , repulsion) 

Cf ree is the set of authorized configurations, k is the dimension of the submanifold where 
the search is conducted. \ k max is the radius of the tubular neighborhood where the search is 
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conducted around the current path. nbu er is the number of iterations of the VDP algorithm, i.e., 
the number of times a submanifold is generated and searched, repulsion is a boolean variable 
set to true if the repulsion parameters have positive values, false if they are all set to zero, 
i.e., there is no repulsion. 

The number \ k max is chosen as a function of the dimension k of the submanifold. Typically, for 
k = 2, }^ max is chosen equal to about 8 times the size of the quantization step d re f. For k = 3, 
\^ max is chosen equal to 4 x d re f. For k = 4, \ A max is chosen equal to d re f. In other words, in 
a 4-dimensional submanifold, the search is only conducted along the immediate neighboring 
configurations of the current path. 

The Progressive Variational Dynamic Programming algorithm can be described as follows. 
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algorithm PVDP (PROGRESSIVE VARIATIONAL DYNAMIC PROGRAMMING) 
begin 

Generation of the initial path 70 using standard VDP planner; 
for i = 1, i < i max , i = i+ 1 

VDP{C} ree , 2, X 2 max , nb iter , true); 

VDP(C^ e , 3, \ 3 max , nb iter , false); 

VDP(C| ee , 4, \ 4 max , nb iter , false); 

if not found Backtrack ; 

endfor; 

end; 



In other words, for each subproblem P,, PVDP performs a few {nbi ter ) iterations of the VDP 
algorithm using 2D submanifolds, then performs a few iterations using 3D submanifolds, and 
finally continues with a few iterations using 4D submanifolds. Of course, if a free path is 
found to problem P, after any of those iterations, the algorithm immediately steps to the next 
subproblem P/ + i . If a valid path for problem P, is not found, the algorithm backtracks, i.e., a 
new initial path 70 is generated using the standard VDP planner for problem Pi, and the PVDP 
procedure is restarted from there. 

The number nbu er is typically set to 5. The search is continued until a free path 7, is found. The 
algorithm is stopped after a solution path 7, mai = 7 is found for the original problem. Since 
the algorithm may never terminate, we artificially impose an upper bound on the total running 
time. The algorithm returns failure if this upper bound is reached. 

Remark: Instead of searching for a better path in a neighborhood of the current path 7 for all 
times t G [0, 1], it is possible to limit the search locally to subintervals of [0, 1] for which 7 
does not satisfy the constraints. This is how the search algorithm has been implemented in the 
PVDP method. 

5.3 Definition of the approximating sequence by a penalty function 

As described in Section 3, the obstacles in the workspace can be represented either using 
geometrical primitives {e.g., polygons), or using distributed representations {e.g., bitmaps). In 
order to define the sequence of free spaces Cj ree , we have chosen to use the representation 
of obstacles by geometrical primitives. Similar algorithms could be defined using bitmap 
representations. 

We assume for the sake of simplicity that obstacles can be described as a finite set of convex 
polygons Bi , . . . , B m . However, our approach can easily be generalized to the case of obstacles 
boundaries represented by higher-order polynomials. Alternatively, the obstacles could be 
represented through a bitmap description, and the following definition of the sequence of free 
spaces could be adapted accordingly. 

Each face B l - of polygon Bj is modeled as an affme function {i.e., a polynomial of degree one) 
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denoted g 1 -. Let hj be the number of faces of Bj. We define: 

Vw G W, gj(w) = min gUw) 
ie[i,hj] 

Polygon Bj can be denned in the following way. 

B j = {weW,g j {w) >0} 
Hence, a point w in the workspace does not intersect with any obstacle iff 

gobst{w) = max gj(w) < 0 

jE[l,m] 

Besides, if the robot considered is articulated, we must check whether or not it collides with 
itself. We assume the set of configurations where the robot does not collide with itself can be 
defined by: 

{q e c , gautocoii{<i) < 0} 

Let X be the forward kinematic map of the robot A. The free space Cp ee being defined as the 
set of configurations such that the robot does not collide with itself or obstacles, we can write: 

Cfree = {qe C, gautocou(<i) < 0} n {q G C, V/? G A, gobst{X{p, q)) < 0} 

We consider a finite decreasing sequence of numbers t\ > ti > ■ ■ ■ > ^i max = 0, and we define 
the corresponding finite sequence of free spaces 

Cfree = {q£ C, g au tocoll{<l) < 0} D {q G C, G A, gobst{X{p, q)) < e,-} 

The sequence e, is called e-strategy. More complex e-strategies can be defined. For example, 
for a given obstacle Bj, the function gj can be replaced by any other function gj: 

ie[i,hj] 

where at\, . . . , aj. are arbitrary positive numbers. Also, any other additional heuristic can be 
added to improve the progressiveness in the sequence of problems Pj. Examples of practical 
e-strategies will be given in Section 6. In general we call e-strategy the whole set of empirical 
parameters that can be used to define the sequence Cj ree . The function g 0 b st is called a penalty 
function, since it is used in the sequence of problems P,- to increasingly penalize the robot 
motions that do not satisfy the obstacle avoidance constraints. 

5.4 Applications of PVDP to manipulation planning problems 

PVDP can be used to address constrained motion planning problems, i.e., extensions of the 
basic path planning problem where the free space in not necessarily an open subset of the 
configuration space. In particular, we have successfully applied PVDP to high-dimensional 
manipulation planning problems. We briefly describe below the extension of the PVDP method 
to manipulation planning problems. A complete presentation of the method can be found in 
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Ferbach and Barraquand 1993 [16]. Given an environment containing a robot, stationary 
obstacles, and a movable object, the manipulation problem consists in finding a sequence of 
free robot motions, grasping and ungrasping operations, to reach a given state from a given 
initial state in the joint configuration space of the robot and of the movable object. The movable 
object can only move when it is grasped by the robot. The generalized obstacles (i.e., forbidden 
postures) in the joint configuration space C are not only the configurations where the robot or 
the movable object hit the stationary obstacles, but also all postures where the movable object 
is levitating without being grasped by the robot. 

It is shown in Ferbach and Barraquand 1993 [16] that under suitable conditions on the set of 
stable configurations for the movable object, the grasping constraints are holonomic, i.e., they 
can be represented by: 

VjG [0 1 l] 1 g gmsp ( 1 (s)) = 0 
where 7 is the path followed by the robot and the movable object. 

Hence, we can define in a fashion similar to that of the previous subsection a decreasing 
sequence of positive numbers e, converging towards zero, and consider the corresponding 
sequence of problems P ; for which the original grasping constraint is replaced by: 

VS G [0, l],ggrasp(l(s)) < e; 

The principle underlying PVDP is to replace the original problem by the series of problems 
Pi. In other words, grasping constraints are handled by PVDP in an iterative fashion. PVDP 
first computes a path where the movable objects can levitate without being grasped by the 
robots. Then, this path is used as the input for a series of increasingly difficult problems where 
the objects must get closer and closer to the robots in order to move. PVDP has successfully 
solved manipulation planning problems of unprecedented complexity. We report in Section 
6 an experiment in dual-arm manipulation task planning for a 12 DOF system. Several other 
examples are described in Ferbach and Barraquand 1993 [16]. 

6 Experimental results 

We have implemented both VDP and PVDP in two programs written in C, running on a 
DEC3000-500 Alpha AXP workstation. We have experimented with VDP and PVDP using a 
variety of robot structures. Several of these experiments are derived from the RPP simulation 
program developed at the Stanford Computer Science Robotics Laboratory (see e.g., Bar- 
raquand and Latombe 1991 [5]). We present below some of the most significant experiments, 
and we compare the capabilities of VDP to that of RPP. 

6.1 10-DOF non-serial manipulator robot in 2D workspace 

We applied VDP to the planar non-serial manipulator robot depicted in Figure 1 , which includes 
three prismatic joints (telescopic links) and seven revolute joints. Figure 1 illustrates a path 
found by VDP for a relatively simple obstacle avoidance problem. 
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Figure 1: VDP method, 2D submanifolds. 




Figure 2: VDP method, 3D submanifolds 
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Figure 4: Various workspaces used in the PVDP method 
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In this example, the dimension of the submanifolds was chosen equal to k = 2. The number of 
iterations of the VDP algorithm was 34. The total computation time was about 3 minutes. This 
is slower than the computation time using the RPP method, which takes about 10 seconds in this 
case. Using 3D submanifolds, VDP finds a path in only 3 iterations instead of 34. However, 
the overall computation time is over 13 minutes, since a single 3D iteration is computationally 
intensive. PVDP solves the same problem in less than 30 seconds. We see that the performance 
of PVDP is comparable to that of RPP on this relatively simple problem. 

We also tested VDP on the more difficult problem depicted in Figure 2. Figure 2 shows a 
path found by VDP using 3D submanifolds. The number of iterations was 76, and the total 
computation time was 7 hours. The VDP method using only 2D submanifolds failed to solve 
this problem. This is dramatically slower than RPP, which solved this problem in about 30 
seconds. Figure 3 shows a path found by PVDP for the same problem. The total computation 
time was 20 minutes. This is much faster than VDP, but still not nearly as fast as RPP. 

We now describe the e-strategy that was used by PVDP for this problem. The original problem 
P was replaced by a sequence of 15 problems P ; . Figure 4 shows a few of the workspaces in 
the series. The length L of the two bars in the middle was chosen according to the following 
formula: 

ViG [1,15], Li = L x + (Lis-Ljy/i/15 

A strictly similar formula was used for the diameter D of the diamond on the left. We see in 
the above formula that the lengths of the obstacles were increased with the square root of the 
problem index i, since the last steps are the most difficult. 

6.2 8-DOF serial manipulator arm in 2D workspace 

We consider the 8-DOF serial manipulator with 8 revolute joints depicted in Figure 5. Figure 
5 shows a path found by VDP using 3D submanifolds. The number of iterations was 93. The 
total computation time was 6 hours. Figure 6 shows a path found by PVDP for the same 
problem. The computation time was 20 minutes. This is still not nearly as fast as RPP, which 
solved the same problem in less than 20 seconds. Figure 7 shows a few of the 40 different 
workspaces used in the progressive method. 

6.3 Coordination of two 3-DOF mobile robots 

The same planner was applied to problems requiring the coordination of two 3-DOF mobile 
robots in a two-dimensional workspace made of several corridors. The problem shown in 
figure 8 is particularly difficult because the two robots have to interchange their positions in 
the central corridor; hence, both of them must first move to an intermediate position in order 
to allow the permutation. Notice that in the initial configuration both robots are rather close to 
their respective goal configurations, however the paths to move there are quite long. 

Figure 8 shows a path found by VDP using 3D submanifolds. The total number of iterations 
was 67, and the computation time 2 hours and 50 minutes. The same problem was solved by 
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Figure 5: VDP method, 3D submanifolds. 




Figure 6: PVDP method, 40 steps. 
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Figure 7: A few of the 40 workspaces used in the PVDP method 
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PVDP in 26 minutes. This is still considerably slower than RPP, which solved this problem in 
less than 20 seconds. 

6.4 16-DOF manipulator robot in 3D workspace 




Figure 9: 3 views of a path found by VDP for a 16DOF manipulator in a 3D workspace. 

VDP was also tested on the 16-DOF manipulator illustrated in Figure 9. This manipulator 
consists of 5 telescopic links connected by 5 spherical joints. The bar at the end of the 
manipulator is connected to the last link by a revolute joint. A path generated by the program 
is illustrated in Figure 9. Three different views (left, center, and right) are given for each of 
the five configurations (from top to bottom) represented along the solution path. The number 
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of iterations of VDP using 3D submanifolds was 20. The total computation time was 2 hours 
and 15 minutes. RPP solved this problem in less than 3 minutes. 

6.5 A manipulation planning problem using two articulated fingers 

The 10-DOF robot depicted in Figure 1 was used in the pick and place problem illustrated 
in Figure 10. We used the PVDP method. The RPP planner is not designed for solving 
manipulation planning problems, hence cannot be compared with PVDP on this example. 

The task assigned to this robot is a simple pick and place operation consisting in grasping the 
disk in the lower right corner of the workspace, bringing it to the lower left corner, and then 
returning to its initial configuration. The total number of degrees of freedom for the whole 
problem is 12. Figure 10 illustrates a manipulation plan found by PVDP. 

In this example, the robot is said to have grasped the disk when the following conditions are 
satisfied: 

• the center M of the disk coincides with the middle R = E '+ E2 of the two end-effectors 
E\ and Ei of the robot. 

• the distance | \E\Ei \ | between the two end effectors E\ and E2 is equal to the diameter D 
of the disk. 

Let Mi and M2 be the initial and goal configurations of the disk. The grasping constraint 
Vf, F(j(t)) = 0 is replaced in the approximating problem P f by the constraint Vf, F(j(t)) < e 
with the following expression for F(q) . 

F(q) = min ( max(||FiF 2 || - D, \\RM\\), min ||MM,-||) 

V " «e{i,2} / 

In other words, in problem P f , either the disk is at distance less than e of a docking position, or 
if satisfies both conditions | |£i2?2| I < D + e and \ \RM\ | < e. 

The initial value of e is one fourth of the size of the workspace. Then, it is decreased at each 
iteration of the penalty function method by 0.001, i.e., 0.1% of the size of the workspace. The 
tolerance value was set to e f0 / = 0.006, i.e 0.6% of the workspace. The path was computed in 
about half an hour. 

7 Discussion and conclusion 

The experiments reported in Section 6 demonstrate that VDP can solve difficult motion planning 
problems with many degrees of freedom. VDP is by far the most reliable and powerful 
variational planner developed to date. But VDP is dramatically slower than potential field 
based planners such as RPP. PVDP is fast for simple problems, but still not nearly as fast as 
RPP for more difficult problems. This is not surprising, since VDP does not use the numerical 
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Figure 10: A pick and place operation using a two-fingered 10-DOF robot 

potential functions that make much of the power of planners such as RPR In the current 
implementation of VDR the submanifolds used for searching collision-free paths are generated 
purely at random. We think it should be possible to use information deriving from potential 
functions in order to improve the procedure generating the submanifolds. Future research 
will determine whether or not VDP can be made as fast as RPP through the use of numerical 
potential functions. 

On the other hand, VDP has several unique features. First, it is a variational planner, and 
can therefore be used in constrained motion planning problems such as manipulation planning 
problems. Such problems are out of reach of classical planners such as RPP. However, as 
outlined in Ferbach and Barraquand 1993 [16], it may be possible to develop a variational 
version of RPP. Future research will determine whether a variational version of RPP can be 
made as efficient as PVDP for solving manipulation planning problems. 

Second, VDP uses dynamic programming. This may become an important advantage over ran- 
domized planners when addressing motion planning problems with non-holonomic constraints. 
Indeed, optimal algorithms based upon dynamic programming already exist (Barraquand and 
Latombe 1993 [6]) for planning motions of non-holonomic mobile robots with few DOF. We 
think it is possible to use the main ideas underlying VDP to develop a motion planner for 
non-holonomic robots with many DOF. This extension is left for future research. 

Third, VDP can be used in cases where the constraints on the solution paths are different 
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from those encountered in classical obstacle avoidance problems. One might think of planning 
problems where the task assigned to the robot is to avoid dangers other than obstacles, such as 
heat or radiation sources. In such a case, the constraint imposed upon the path is not binary but 
real valued. For example, the robot's task may be to minimize along its path the accumulated 
heat or radiation level. Then, the minimum cost functional J of VDP can be easily extended 
to take into account such real-valued constraints. More generally, VDP can be viewed as a 
systematic technique for addressing optimal control problems for high-dimensional holonomic 
dynamical systems. Its possible applications extend far beyond those in robotics, to many other 
fields of control theory. 
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